#include "serial_node/serial_node.hpp"

Serial_Node::Serial_Node(const char* port_name,int baudrate,int timeout)
{
    ser.setPort(port_name);
    ser.setBaudrate(baudrate);
    serial::Timeout to=serial::Timeout::simpleTimeout(timeout);
    ser.setTimeout(to);

    tim=n.createTimer(ros::Duration(0.005),&Serial_Node::tim_callback,this);
    cmd_sub=n.subscribe("cmd_vel",10,&Serial_Node::tx_callback,this);
    
    
    while(1)
    {
        try
        {
            ser.open();
            if(ser.isOpen())
            {
                ROS_INFO("Port open success.");
                break;
            }
        }
        catch(const std::exception& e)
        {
            ROS_ERROR("Port open failed.");
        }
    }
}
void Serial_Node::tx_callback(const geometry_msgs::Twist &tx_msg)
{
    try
    {
        txx.header=tx_header;
        txx.Vx = tx_msg.linear.x;
        txx.Vy = tx_msg.linear.y;
        txx.Wz = tx_msg.angular.z;
        Append_CRC16_Check_Sum((uint8_t*)&txx,tx_len);
        ser.write((uint8_t*)&txx,tx_len);
        /* ROS_INFO("tx_header%d",txx->header);
        for(int i=0; i<tx_len; i++)
        {
        //16进制的方式打印到屏幕
            std::cout << std::hex << (((uint8_t*)txx)[i] & 0xff) << " ";
        } */
    }
    catch(const std::exception& e)
    {
        
    }
}
void Serial_Node::tim_callback(const ros::TimerEvent& e)
{
    if(ros::ok())
    {
        if(ser.isOpen()==false)
        {
            try
            {
                ser.open();
            }
            catch(const std::exception& e)
            {
            }
            return;
        }
        try
        {
            uint16_t buffer_len=ser.available();
            if(buffer_len>=rx_len*2)
            {
                ser.read((uint8_t*)rx_buffer,rx_len*2);   
                for(int i=0;i<rx_len+1;i++)
                {
                    if(rx_buffer[i]==rx_header)
                    {
                        
                        if(Verify_CRC16_Check_Sum((uint8_t*)&rx_buffer[i],rx_len))
                        {
                            memcpy((uint8_t*)&rxx,&rx_buffer[i],rx_len);
                            /* ROS_INFO("rx_header%d",rxx->header);
                            for(int i=0; i<rx_len; i++)
                            {
                                //16进制的方式打印到屏幕
                                std::cout << std::hex << (((uint8_t*)rxx)[i] & 0xff) << " ";
                            } */
                            ROS_INFO("rx_header%d",rxx.header);
                        }
                        else
                        {
                            ROS_ERROR("CRC false.");
                        }           
                    }
                }
            }
        }
        catch(serial::IOException &e)
        {
            ser.close();
            try
            {
                ser.open();
            }
            catch(const std::exception& e)
            {
                
            }
        }
    }
}
Serial_Node::~Serial_Node()
{
    ser.close();
}

int main(int argc,char** argv)
{
    ros::init(argc,argv,"serial_node");
    ros::AsyncSpinner spinner(2);
    spinner.start();
    Serial_Node serial_node("/dev/ttyACM0",115200,10000);
    ros::waitForShutdown();
    ROS_ERROR("serial closing...");
    return 0;
}
